//
// Created by PulsarV on 19-2-26.
//

#ifndef USE_DSP_DEVICE
#include <rc_move/rcmove.h>
#include <rc_gpio/c_gpio.h>
#include <rc_gpio/soft_pwm.h>
#include <base/slog.hpp>

namespace RC {
    void RobotCarMove::wheel_1_backward(int speed) {
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_1);
        RC::GPIO::pwm_set_frequency(rcMoveDevice.pwm_1, speed);
        RC::GPIO::digitalWrite(rcMoveDevice.gpio_1, RC_GPIO_HIGH);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_1);
    }

    void RobotCarMove::wheel_1_forward(int speed) {
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_1);
        RC::GPIO::pwm_set_frequency(rcMoveDevice.pwm_1, speed);
        RC::GPIO::digitalWrite(rcMoveDevice.gpio_1, RC_GPIO_LOW);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_1);
    }

    void RobotCarMove::wheel_2_backward(int speed) {
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_2);
        RC::GPIO::pwm_set_frequency(rcMoveDevice.pwm_2, speed);
        RC::GPIO::digitalWrite(rcMoveDevice.gpio_2, RC_GPIO_HIGH);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_2);
    }

    void RobotCarMove::wheel_2_forward(int speed) {
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_2);
        RC::GPIO::pwm_set_frequency(rcMoveDevice.pwm_2, speed);
        RC::GPIO::digitalWrite(rcMoveDevice.gpio_2, RC_GPIO_LOW);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_2);
    }

    void RobotCarMove::wheel_3_backward(int speed) {
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_3);
        RC::GPIO::pwm_set_frequency(rcMoveDevice.pwm_3, speed);
        RC::GPIO::digitalWrite(rcMoveDevice.gpio_3, RC_GPIO_HIGH);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_3);
    }

    void RobotCarMove::wheel_3_forward(int speed) {
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_3);
        RC::GPIO::pwm_set_frequency(rcMoveDevice.pwm_3, speed);
        RC::GPIO::digitalWrite(rcMoveDevice.gpio_3, RC_GPIO_LOW);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_3);
    }

    void RobotCarMove::wheel_AC(int speed_1, int speed_2, int speed_3) {
        wheel_1_forward(speed_1);
        wheel_2_forward(speed_2);
        wheel_3_forward(speed_3);
    }

    void RobotCarMove::wheel_CW(int speed_1, int speed_2, int speed_3) {
        wheel_1_backward(speed_1);
        wheel_2_backward(speed_2);
        wheel_3_backward(speed_3);
    }

    void RobotCarMove::wheel_go_forward(int speed_1, int speed_2) {
        wheel_stop();
        wheel_1_backward(speed_1);
        wheel_2_forward(speed_2);
    }

    void RobotCarMove::wheel_go_backward(int speed_1, int speed_2) {
        wheel_stop();
        wheel_1_forward(speed_1);
        wheel_2_backward(speed_2);
    }

    void RobotCarMove::wheel_start() {
        RC::GPIO::pwm_start(rcMoveDevice.pwm_1);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_2);
        RC::GPIO::pwm_start(rcMoveDevice.pwm_3);
    }

    void RobotCarMove::wheel_stop() {
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_1);
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_2);
        RC::GPIO::pwm_stop(rcMoveDevice.pwm_3);
    }

    void RobotCarMove::wheel_check(int times) {

    }

    bool flag = 0;

    void RobotCarMove::excute(WHEEL_DATA wheelData) {
//        slog::info << "wheel speed "
//                   << wheelData.wheel_1_speed << " " << wheelData.weel_1_direction << " "
//                   << wheelData.wheel_2_speed << " " << wheelData.weel_2_direction << " "
//                   << wheelData.wheel_3_speed << " " << wheelData.weel_3_direction << " "
//                   << slog::endl;
        wheel_stop();

        if (wheelData.wheel_1_speed != 0) {
            if (wheelData.weel_1_direction == RC_MOVE_FORWARD) {
                wheel_1_forward(wheelData.wheel_1_speed);
            } else
                wheel_1_backward(wheelData.wheel_1_speed);
        }
        if (wheelData.wheel_2_speed != 0) {
            if (wheelData.weel_2_direction == RC_MOVE_FORWARD) {
                wheel_2_forward(wheelData.wheel_2_speed);
            } else
                wheel_2_backward(wheelData.wheel_2_speed);
        }

        if (wheelData.wheel_3_speed != 0) {
            if (wheelData.weel_3_direction == RC_MOVE_FORWARD) {
                wheel_3_forward(wheelData.wheel_3_speed);
            } else
                wheel_3_backward(wheelData.wheel_3_speed);
        }
    }
}

#endif
